#include <ros/ros.h>
#include <std_msgs/Float64.h>

namespace inv_pen
{
    class AccPub
    {
    public:
        AccPub(std::string ns);
        void DxSubCallBack(const std_msgs::Float64 &msg);
        void ThetaSubCallBack(const std_msgs::Float64 &msg);
        void SituCmdSubCallBack(const std_msgs::Float64 &msg);
        void loop(void);
        double get_rate(void);
    private:
        double dx;
        double theta;
        double error;
        double rate;
        ros::NodeHandle pnh;
        std_msgs::Float64 situ_acc;
        std_msgs::Float64 angle_acc;
        double M, m, b, l, J, g;
        ros::Publisher situ_acc_pub;
        ros::Publisher angle_acc_pub;
        ros::Subscriber dx_sub;
        ros::Subscriber theta_sub;
        ros::Subscriber situ_cmd_sub;
    };
}

inv_pen::AccPub::AccPub(std::string ns) : dx(0.0), theta(0.0), error(0.0),rate(25)
{
    situ_acc.data = 0.0;
    angle_acc.data = 0.0;
    pnh = ros::NodeHandle(ns);
    ros::param::get("/m", m);
    ros::param::get("/M", M);
    ros::param::get("/b", b);
    ros::param::get("/l", l);
    ros::param::get("/J", J);
    ros::param::get("/g", g);
    situ_acc_pub = pnh.advertise<std_msgs::Float64>("/situ_acc", 1);
    angle_acc_pub = pnh.advertise<std_msgs::Float64>("/angle_acc", 1);
    dx_sub = pnh.subscribe("/situ_vel", 2, &inv_pen::AccPub::DxSubCallBack, this);
    theta_sub = pnh.subscribe("/theta", 2, &inv_pen::AccPub::ThetaSubCallBack, this);
    situ_cmd_sub = pnh.subscribe("/situ_cmd", 2, &inv_pen::AccPub::SituCmdSubCallBack, this);
}

void inv_pen::AccPub::DxSubCallBack(const std_msgs::Float64 &msg)
{
    dx = msg.data;
    // check
    // ROS_INFO("recieve dx as: %f",dx);
}

void inv_pen::AccPub::ThetaSubCallBack(const std_msgs::Float64 &msg)
{
    theta = msg.data;
    // ROS_INFO("recieve theta as: %f",theta);
}

void inv_pen::AccPub::SituCmdSubCallBack(const std_msgs::Float64 &msg)
{
    error = msg.data;
    // ROS_INFO("recieve error as: %f",error);
}

void inv_pen::AccPub::loop(void)
{
    ros::spinOnce();
    /*
    相当于接收的situ-cmd就是输入u
    */
    double pol = (M * m * l * l + J * (m + M));
    double tmp = (J * b - b * m * l * l) * dx / pol + g * m * m * l * l * theta / pol + (J + m * l * l) * error / pol;
    situ_acc.data = tmp;
    tmp = b * m * l * dx / pol - (m + M) * l * m * g * theta / pol - m * l * error / pol;
    angle_acc.data = tmp;
    situ_acc_pub.publish(situ_acc);
    angle_acc_pub.publish(angle_acc);
    ROS_INFO("pub acc: %f\t%f", situ_acc.data,angle_acc.data);
    
    
    //check,correct
    // ROS_INFO_ONCE("a22:%f\ta23:%f\ta42:%f\ta43:%f",(J * b - b * m * l * l)/ pol,g * m * m * l * l / pol, b * m * l / pol,-1* (m + M) * l * m * g / pol  );
    // ROS_INFO_ONCE("b2:%f\tb4:%f", (J + m * l * l) / pol,-1*m * l  / pol);
}

double inv_pen::AccPub::get_rate(void)
{
    return rate;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "acc_pub_node");
    inv_pen::AccPub accpub("");

    ros::Rate rate(accpub.get_rate());
    while (ros::ok())
    {
        accpub.loop();
        rate.sleep();
    }
    ros::shutdown();
    return 0;
}